#include "includes.h"
#include <app_cfg.h>
#include "misc.h"
#include "stm32f4xx_it.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_rcc.h"
#include "timer.h"
#include "gpio.h"
#include "usart.h"
#include "can.h"
#include "elmo.h"
#include "pps.h"
#include "moveBase.h"
#include "fort.h"
#include "runshoot.h"
#include "moveprote.h"	

 typedef union{
			float dataf;
			uint8_t data8[4];
	}data;
	data z_angle;
	data z_w;
	data x_vel;
	data y_vel;
	data x_pos;
	data y_pos;
void TestPPS(void);   //log the trace
void TestShooter(void);
extern char USART_RX[];		//串口发送消息
extern int USART_Flag,yawFinishFlag,aimActualPosFlag,correctFlag;  //串口标志位，航向完成标志位
extern int32_t actual1Vel,actual2Vel,actual7Pos,actual7Vel,actual5Vel,actual6Vel;		//读取到的电机速度位置
extern float shooterVel,yawPos,yawAgl;  //摩擦轮转速 r/s 航向位置 给定航向角
extern char ballColor[]; // 0无，1白，2黑，3粉  直接当作int型变量使用
extern int goBackFlag;   //逃逸
extern float detv1;  //变半径圆
extern int detr1;  
int x,y,speedX,speedY,angleZ,xShoot,yShoot;					//坐标;
float angle;  
int vCheckL,vCheckR,mode=6;  
int checkCanErr=0;
float testRps=25;         	//给定的摩擦轮转速，r/s
extern int keyOneStatus,keyTwoStatus,keyThreeStatus,keyFourStatus,keyRedBlueStatus,keyWolkingStatus,keyCheckStatus;   //switch status
int date[6];
float correctDetX=0,correctDetY=0,correctDetAgl=0;   // corrdinate error amount
int goUpFlag=0;  //init car flag 
int mode5Flag=0,usartCrashMod;
extern int blueBallStore,redBallStore,getBallFlag,correctTimeCnt;
extern int pinkColorFlag;
extern int motorActualPos,crashTimes,vTime;
extern float shootRaps;
extern int colorChangeFlag;
extern fort_t fort;
int checktemp1,checktemp2,checktemp3;
int temp9,correctStopAgl=0,mode5StopAgl;
int usartCorrectTimeCnt=0,usartCorrectTimes=0,lastMode=0,usartBallNothingCnt=0,usartMotorActualPos=0,usartCoordinateOne=0,usartCoordinateTwo=0,usartAngleThree=0;
int usartModFlag=0,usartTimeCntColorTwo,usartTimeCntColorOne,usartStatus;
int giveBallTime=0;
int temp1,temp2,temp3,temp4,temp5,temp6,temp7,temp8;
int ballNum5=0,ballNum6=0,coloect5BallFlag=0,coloect6BallFlag=0;
/*
===============================================================
						信号量定义
===============================================================
*/
OS_EXT INT8U OSCPUUsage;
OS_EVENT *PeriodSem;

static OS_STK App_ConfigStk[Config_TASK_START_STK_SIZE];
static OS_STK WalkTaskStk[Walk_TASK_STK_SIZE];

void App_Task()
{
	CPU_INT08U os_err;
	os_err = os_err; /*防止警告...*/

	/*创建信号量*/
	PeriodSem = OSSemCreate(0);

	/*创建任务*/
	os_err = OSTaskCreate((void (*)(void *))ConfigTask, /*初始化任务*/
						  (void *)0,
						  (OS_STK *)&App_ConfigStk[Config_TASK_START_STK_SIZE - 1],
						  (INT8U)Config_TASK_START_PRIO);

	os_err = OSTaskCreate((void (*)(void *))WalkTask,
						  (void *)0,
						  (OS_STK *)&WalkTaskStk[Walk_TASK_STK_SIZE - 1],
						  (INT8U)Walk_TASK_PRIO);
}

/*
   ===============================================================
   初始化任务
   ===============================================================
   */
void ConfigTask(void)
{
	CPU_INT08U os_err;
	os_err = os_err;
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);	
	USART2_Init(921600);//与炮台主控
	USART3_Init(115200);//串口3和定位系统相连，接收定位系统的数据
	USART1_Init(921600);	//蓝牙收数
	USART6_Init(115200);    //反馈定位系统数据
//	BeepInit();         //行程开关及蜂鸣器初始化
	KeyInit();
	CAN_Config(CAN1, 500, GPIOB, GPIO_Pin_8, GPIO_Pin_9);
	CAN_Config(CAN2, 500, GPIOB, GPIO_Pin_5, GPIO_Pin_6);
	TIM_Init(TIM2, 99, 839, 1, 0);
//	ElmoInit(CAN1);
//	ElmoInit(CAN2);
//	VelLoopCfg(CAN1, 1, 32768*50, 32768*50);
//	VelLoopCfg(CAN1, 2, 32768*50, 32768*50);
//	VelLoopCfg(CAN2, 5, 32768*100, 32768*100);
//	VelLoopCfg(CAN2, 6, 32768*100, 32768*100);
//	PosLoopCfg(CAN2,7,32768*2000, 32768*2000,32768*350);
	MotorOff(CAN1,1);
	MotorOff(CAN1,2);
	for(uint8_t i = 0 ; i <=10 ; i++)
	{
		delay_ms(1000);
	}
	WaitOpsPrepare();
	OSTaskSuspend(OS_PRIO_SELF);
}

void WalkTask(void)
{
	CPU_INT08U os_err;
	os_err = os_err;
	OSSemSet(PeriodSem, 0, &os_err);
	while (1)
	{		
		OSSemPend(PeriodSem, 0, &os_err);
		OSSemSet(PeriodSem, 0, &os_err);	
		if(goUpFlag==0)
		{
			VelCrl(CAN2,5,-32768*75);
			VelCrl(CAN2,6,-32768*75);
			InitCar();
		}
		else
		{
//			ReadActualVel(CAN1,1); 
//			ReadActualVel(CAN1,2);
//			ReadActualVel(CAN2,7);
			ReadActualVel(CAN2,5);
			ReadActualVel(CAN2,6);
			angle=GetAngle()+correctDetAgl;   //getPPS
			if(angle<-180)		angle+=360;
			else if(angle>180)	 angle-=360;
			x=GetX()+correctDetX;
			y=GetY()+correctDetY;
			speedX=GetSpeedX();
			speedY=GetSpeedY();
			angleZ= GetWZ();	
			
			z_angle.dataf=angle;
      z_w.dataf=GetWZ();
      x_vel.dataf=GetSpeedX();
      y_vel.dataf=GetSpeedY();
      x_pos.dataf=x+45.7*sin(angle*PI/180);
      y_pos.dataf=y+45.7*(1-cos(angle*PI/180));	
			
			xShoot=(int)(x+86.85*sin(angle*PI/180.f));
			yShoot=(int)(y+86.85*(1-cos(angle*PI/180)));			
			x=(int)(x+170.7*sin(angle*PI/180));
			y=(int)(y+170.7*(1-cos(angle*PI/180)));
			if(keyCheckStatus==1)	
			{
				if(keyOneStatus==1&&keyTwoStatus==1&&keyFourStatus==1&&keyThreeStatus==1)
				{
					ClearBall();
				}
				else
				{
					SelfCheck();
				}
			}
			else
			{
				static int selectBallFlag=1;
				if(selectBallFlag)
				{
					redBallStore=backBallStore(redBallStore);
					blueBallStore=backBallStore(blueBallStore);
					selectBallFlag=0;
				}
				else
				{
//					USART_OUT(USART1,"\t%d\t",(int)fort.cameraReceive.color);
//					USART_OUT(USART1,"\t%d\t",(int)fort.cameraReceive.status);					
//					USART_OUT(USART1,"\t%d\t",(int)fort.cameraReceive.disOne);	
//					USART_OUT(USART1,"\t%d\t",(int)fort.cameraReceive.disTwo);				
//					USART_OUT(USART1,"\t%d\t",(int)(fort.cameraReceive.angle*100));
//					USART_OUT(USART1,"L\t%d\t",(-1*actual1Vel));
//					USART_OUT(USART1,"R\t%d\t",(-1*actual2Vel));
//					USART_OUT(USART1,"CrctFlg\t%d\t",correctFlag);      //是否进行坐标矫正
//					USART_OUT(USART1,"CrctTimCnt\t%d\t", usartCorrectTimeCnt);   //矫正计时
//					USART_OUT(USART1,"BldFlg\t%d\t",usartBlindFlag);      //是否只看到条边
//					USART_OUT(USART1,"correctTims\t%d\t",usartcorrectTimes);   //看不到角的次数
//					USART_OUT(USART1,"dtX\t%d\t",(int)correctDetX);        //坐标矫正量
//					USART_OUT(USART1,"dtY\t%d\t",(int)correctDetY);	
//					USART_OUT(USART1,"dtA\t%d\t", (int)correctDetAgl);
//					USART_OUT(USART1,"one\t%d\t",(int)usartCoordinateOne);       
//					USART_OUT(USART1,"two\t%d\t",(int)usartCoordinateTwo);	
//					USART_OUT(USART1,"thr\t%d\t", (int)usartAngleThree);
//					USART_OUT(USART1,"thr\t%d\t", (int)usartStatus);
//					USART_OUT(USART1,"cnt1\t%d\t", usartTimeCntColorOne);    
//					USART_OUT(USART1,"cnt2\t%d\t",usartTimeCntColorTwo);	
//					USART_OUT(USART1,"detr1\t%d\t", (int)detr1);    //变半径大小
//					USART_OUT(USART1,"goBckFlg\t%d\t",goBackFlag);		  //是否逃逸	
//					USART_OUT(USART1,"CrshMod\t%d\t",usartCrashMod);  //碰撞的模式
//					USART_OUT(USART1,"crshtims\t%d\t",crashTimes);		            //碰撞次数
//					USART_OUT(USART1,"vTims\t%d\t",vTime);		                //碰撞计时
////					USART_OUT(USART1,"modflag\t%d\t",usartModFlag);   //扫边的标志位
					USART_OUT(USART1,"X\t%d\t",x);
					USART_OUT(USART1,"Y\t%d\t",y);
//					USART_OUT(USART1,"Agl\t%d\t",(int)(angle*100));
////					USART_OUT(USART1,"XS:\t%d\t",xShoot);
////					USART_OUT(USART1,"YS:\t%d\t",yShoot);	
//					USART_OUT(USART1,"XSpd:\t%d\t",speedX);
//					USART_OUT(USART1,"YSpd:\t%d\t",speedY);
//					USART_OUT(USART1,"WZ:\t%d\t",angleZ);	
//					USART_OUT(USART1,"CLR1:\t%d\t",(int)ballColor[0]);
//					USART_OUT(USART1,"CLR2:\t%d\t",(int)ballColor[1]);				
//////					USART_OUT(USART1,"ballNothingCnt\t%d\t",usartBallNothingCnt); //无球计时
////////					USART_OUT(USART1,"pinkcolorflag\t%d\t",pinkColorFlag);
//					USART_OUT(USART1,"5V\t%d\t",(int)(actual5Vel));
//					USART_OUT(USART1,"6V\t%d\t",(int)(actual6Vel));
////					USART_OUT(USART1,"7V\t%d\t",(int)(actual7Vel));
//					USART_OUT(USART1,"5F\t%d\t",(int)(coloect5BallFlag));
//					USART_OUT(USART1,"6F\t%d\t",(int)(coloect6BallFlag));
//					USART_OUT(USART1,"5Cnt\t%d\t",ballNum5);
//					USART_OUT(USART1,"6Cnt\t%d\t",ballNum6);
					USART_OUT(USART1,"shterV\t%d\t",(int)(100*shooterVel));
					USART_OUT(USART1,"shterRaps\t%d\t",(int)(100*shootRaps));
					USART_OUT(USART1,"detRaps\t%d\t",(int)(100*(shootRaps-shooterVel)));
//////					USART_OUT(USART1,"givenRps\t%d\r\n",(int)(100*testRps));
					USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
					USART_OUT(USART1,"yawAgl\t%d\t",(int)(100*yawAgl));
//					USART_OUT(USART1,"yaFinishflag\t%d\t",yawFinishFlag);
//////					USART_OUT(USART1,"colorchange\t%d\t",colorChangeFlag);
//					USART_OUT(USART1,"aim\t%d\t",aimActualPosFlag);
//					USART_OUT(USART1,"motor:\t%d\t",motorActualPos);
//					USART_OUT(USART1,"7Pos\t%d\t",actual7Pos);
//					USART_OUT(USART1,"\t%d\t",temp1);
//					USART_OUT(USART1,"\t%d\t",temp2);
//					USART_OUT(USART1,"\t%d\t",temp3);
//					USART_OUT(USART1,"\t%d\t",temp4);
//					USART_OUT(USART1,"\t%d\t",temp5);
//					USART_OUT(USART1,"\t%d\t",temp6);
//					USART_OUT(USART1,"\t%d\t",temp7);
//					USART_OUT(USART1,"\t%d\t",temp8);
////					USART_OUT(USART1,"gbalFlg\t%d\t",getBallFlag);
//////					USART_OUT(USART1,"timectwo:\t%d\t",checktemp1);
//////					USART_OUT(USART1,"check2\t%d\t",checktemp2);
//////					USART_OUT(USART1,"chect3\t%d\t",checktemp3);
					USART_OUT(USART1,"mod\t%d\r\n",mode);  //行走方式
					if(USART_Flag)                                                                     //usart
					{
						USART_Flag=0;
						if(USART_RX[0]=='0')
						{
//							PosCrl(CAN2,7,ABSOLUTE_MODE,0);
							correctFlag=1;
//							testRps-=0.2;
						}
//						if(USART_RX[0]=='1')
//						{
//							PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
//							correctFlag=1;
////							testRps-=0.2;
//						}
					}
					else
					{						
						if(correctFlag==1)
						{
							Correct();
						}
						else
						{
							TestShooter();
							if(actual6Vel>2400000)
							{
								coloect6BallFlag=0;
							}
							if(actual5Vel>2400000)
							{
								coloect5BallFlag=0;
							}
							if((actual5Vel<2200000&&actual5Vel>1500000))   //检测到收球滚子进球
							{
								if(coloect5BallFlag==0)
									ballNum5++;
								coloect5BallFlag=1;
							}
							if(actual6Vel<2200000&&actual6Vel>1500000)
							{
								if(coloect6BallFlag==0)
									ballNum6++;
								coloect6BallFlag=1;
							}
						}
//						PidRoundOri(1.6,1200,0);
////						keyRedBlueStatus=0;  //选场，红1蓝0
//						
////						redBallStore=1;  //选桶 1 2
////						redBallStore=4;  //  2 3
////						redBallStore=5;  // 2 4
//						
////						blueBallStore=1;  //选桶  12
////						blueBallStore=4;  //2 3
////						blueBallStore=5;  //2 4
//						
////						keyWolkingStatus=4;	 //走形
//						checkCrash();
//						if(mode!=5)
//						{	
//							lastMode=mode;
//							mode5Flag=1;
//						}
//						if(correctFlag==2)   // 在停下的途中被撞了
//						{
//							correctTimeCnt=0;
//							correctFlag=0;
//							mode=lastMode;
//							if(mode==6)	mode=0;
//						}
//						else if(correctFlag==1/*||correctFlag==3*/) //运动中被撞了一次或者投球的时候
//						{
//	//						Correct();
//							if((detr1==0||detr1==350)&&mode==5) //说明投球前的矫正 ，第一圈不要
//							{
//								correctFlag=0;
//							}
//							else 
//							{
//								Correct();
//							}			
//						}
//						else 
//						{
//							if((actual6Vel<2133760&&actual6Vel>1500000)||(actual5Vel<2133760&&actual5Vel>1500000))   //检测到收球滚子进球
//							{
//								getBallFlag=1;
//							}
//							StopAndShoot();
//							correctStopAgl=angle;
//						}	
////						TestPPS();	
					}
				}
			}
		}
	}
}

void TestPPS(void)
{
	 USART_SendData(USART1,0x0d);
	 USART_SendData(USART1,0x0a);
	 
	 USART_SendData(USART1,z_angle.data8[0]);
	 USART_SendData(USART1,z_angle.data8[1]);
	 USART_SendData(USART1,z_angle.data8[2]);
	 USART_SendData(USART1,z_angle.data8[3]);	
	 
	 USART_SendData(USART1,z_w.data8[0]);
	 USART_SendData(USART1,z_w.data8[1]);
	 USART_SendData(USART1,z_w.data8[2]);
	 USART_SendData(USART1,z_w.data8[3]);
		
	 USART_SendData(USART1,x_vel.data8[0]);
	 USART_SendData(USART1,x_vel.data8[1]);
	 USART_SendData(USART1,x_vel.data8[2]);
	 USART_SendData(USART1,x_vel.data8[3]);
	 		
	 USART_SendData(USART1,y_vel.data8[0]);
	 USART_SendData(USART1,y_vel.data8[1]);
	 USART_SendData(USART1,y_vel.data8[2]);
	 USART_SendData(USART1,y_vel.data8[3]);

	 USART_SendData(USART1,x_pos.data8[0]);
	 USART_SendData(USART1,x_pos.data8[1]);
	 USART_SendData(USART1,x_pos.data8[2]);
	 USART_SendData(USART1,x_pos.data8[3]);
	 
	 USART_SendData(USART1,y_pos.data8[0]);
	 USART_SendData(USART1,y_pos.data8[1]);
	 USART_SendData(USART1,y_pos.data8[2]);
	 USART_SendData(USART1,y_pos.data8[3]);
	 
	 USART_SendData(USART1,0x0a);
	 USART_SendData(USART1,0x0d);
}
void TestShooter(void)
{
	angle-=1.5;
	mode=5;
	collectBall();
	static int shootDistance;
	if(USART_RX[0]=='5')
	{
			CenterAim();
			shootDistance=fabs(sqrt(xShoot*xShoot+(yShoot-2241)*(yShoot-2241)));
			shootDistance+=158.85f;
			if(shootDistance>=700&&shootDistance<800)shootRaps=30.8f+(shootDistance-700)*0.024f;
			else if(shootDistance<900)shootRaps=33.2f+(shootDistance-800)*0.02f;
			else if(shootDistance<1100)shootRaps=35.2f+(shootDistance-900)*0.024f;
			else if(shootDistance<1200)shootRaps=40.f+(shootDistance-1100)*0.028f;
			else if(shootDistance<1500)shootRaps=42.8f+(shootDistance-1200)*0.02f;
			else if(shootDistance<1700)shootRaps=48.8f+(shootDistance-1500)*0.016f;
			else if(shootDistance<1800)shootRaps=52.f+(shootDistance-1700)*0.018f;
			else if(shootDistance<1900)shootRaps=53.8f+(shootDistance-1800)*0.02f;
			else if(shootDistance<2000)shootRaps=55.8f+(shootDistance-1900)*0.016f;
			else if(shootDistance<2100)shootRaps=57.4f+(shootDistance-2000)*0.014f;
			else if(shootDistance<2200)shootRaps=58.8f+(shootDistance-2100)*0.013f;
			else if(shootDistance<2300)shootRaps=60.1f+(shootDistance-2200)*0.011f;
			else shootRaps=(-4.163e-6*shootDistance*shootDistance)+0.03176f*shootDistance+10.34f;
	}
	else if(USART_RX[0]=='1')
	{
		if(keyRedBlueStatus==0)
		{
			BlueOne();
			shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
			shootDistance+=358.85f;
		}
		else
		{
			RedOne();
			shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
			shootDistance+=358.85f;
		}
		
		if(shootDistance>=900&&shootDistance<1000)shootRaps=39.8f+(shootDistance-900)*0.008f;
			else if(shootDistance<1100)shootRaps=40.6f+(shootDistance-1000)*0.012f;
			else if(shootDistance<1200)shootRaps=41.8f+(shootDistance-1100)*0.022f;
			else if(shootDistance<1400)shootRaps=44.f+(shootDistance-1200)*0.014f;
			else if(shootDistance<1500)shootRaps=46.8f+(shootDistance-1400)*0.018f;
			else if(shootDistance<1600)shootRaps=48.6f+(shootDistance-1500)*0.02f;
			else if(shootDistance<1700)shootRaps=50.6f+(shootDistance-1600)*0.016f;
			else if(shootDistance<1800)shootRaps=52.2f+(shootDistance-1700)*0.014f;
			else if(shootDistance<1900)shootRaps=53.6f+(shootDistance-1800)*0.018f;
			else if(shootDistance<2000)shootRaps=55.4f+(shootDistance-1900)*0.016f;
			else if(shootDistance<2100)shootRaps=57.f+(shootDistance-2000)*0.014f;
			else if(shootDistance<2200)shootRaps=58.4f+(shootDistance-2100)*0.016f;
			else if(shootDistance<2300)shootRaps=60.f+(shootDistance-2200)*0.014f;
			else if(shootDistance<2400)shootRaps=61.4f+(shootDistance-2300)*0.012f;
			else if(shootDistance<2500)shootRaps=62.6f+(shootDistance-2400)*0.01f;
			else if(shootDistance<2600)shootRaps=63.6f+(shootDistance-2500)*0.016f;
			else if(shootDistance<2800)shootRaps=65.2f+(shootDistance-2600)*0.01f;
			else if(shootDistance<3000)shootRaps=67.2f+(shootDistance-2800)*0.012f;
			else if(shootDistance<3100)shootRaps=69.6f+(shootDistance-3000)*0.009f;
			else if(shootDistance<3200)shootRaps=70.5f+(shootDistance-3100)*0.013f;
			else if(shootDistance<3300)shootRaps=71.8f+(shootDistance-3200)*0.008f;
			else if(shootDistance<3400)shootRaps=72.6f+(shootDistance-3300)*0.014f;
			else if(shootDistance<3700)shootRaps=74.f+(shootDistance-3400)*0.012f;
			else if(shootDistance<3800)shootRaps=77.6f+(shootDistance-3700)*0.008f;
			else if(shootDistance<3900)shootRaps=78.4f+(shootDistance-3800)*0.014f;
			else if(shootDistance<4000)shootRaps=79.8f+(shootDistance-3900)*0.006f;
			else if(shootDistance<4100)shootRaps=80.4f+(shootDistance-4000)*0.011f;
			else if(shootDistance<4200)shootRaps=81.5f+(shootDistance-4100)*0.013f;
			else if(shootDistance<4400)shootRaps=82.8f+(shootDistance-4200)*0.01f;
			else if(shootDistance<4500)shootRaps=84.8f+(shootDistance-4400)*0.014f;
			else shootRaps=(-1.163e-6*shootDistance*shootDistance)+0.01911f*shootDistance+22.91f;
	}
	else if(USART_RX[0]=='2')
	{
		if(keyRedBlueStatus==0)
		{
			BlueTwo();
		shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));		
		shootDistance+=358.85f;
		}
		else
		{
			RedTwo();
		shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));		
		shootDistance+=358.85f;
		}
		
		if(shootDistance>=1400&&shootDistance<1500)shootRaps=49.6f+(shootDistance-1400)*0.016f;
			else if(shootDistance<1700)shootRaps=51.2f+(shootDistance-1500)*0.018f;
			else if(shootDistance<1800)shootRaps=54.8f+(shootDistance-1700)*0.012f;
			else if(shootDistance<1900)shootRaps=56.f+(shootDistance-1800)*0.018f;
			else if(shootDistance<2000)shootRaps=57.8f+(shootDistance-1900)*0.014f;
			else if(shootDistance<2200)shootRaps=59.2f+(shootDistance-2000)*0.016f;
			else if(shootDistance<2400)shootRaps=62.4f+(shootDistance-2200)*0.014f;
			else if(shootDistance<2500)shootRaps=65.2f+(shootDistance-2400)*0.008f;
			else if(shootDistance<2800)shootRaps=66.f+(shootDistance-2500)*0.012f;
			else if(shootDistance<2900)shootRaps=69.6f+(shootDistance-2800)*0.006f;
			else if(shootDistance<3000)shootRaps=70.2f+(shootDistance-2900)*0.01f;
			else if(shootDistance<3100)shootRaps=71.2f+(shootDistance-3000)*0.006f;
			else if(shootDistance<3200)shootRaps=71.8f+(shootDistance-3100)*0.018f;
			else if(shootDistance<3500)shootRaps=73.6f+(shootDistance-3200)*0.01f;
			else if(shootDistance<3600)shootRaps=76.6f+(shootDistance-3500)*0.012f;
			else if(shootDistance<3700)shootRaps=77.8f+(shootDistance-3600)*0.014f;
			else if(shootDistance<3800)shootRaps=79.2f+(shootDistance-3700)*0.012f;
			else if(shootDistance<3900)shootRaps=80.4f+(shootDistance-3800)*0.006f;
			else if(shootDistance<4100)shootRaps=81.f+(shootDistance-3900)*0.01f;
			else if(shootDistance<4300)shootRaps=83.f+(shootDistance-4100)*0.012f;
			else shootRaps=(-1.173e-6*shootDistance*shootDistance)+0.01856f*shootDistance+26.56f;
	}
	else if(USART_RX[0]=='3')
	{
		if(keyRedBlueStatus==0)
		{
				BlueThree();
		shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
		shootDistance+=358.85f;
		}
		else
		{
				RedThree();
		shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
		shootDistance+=358.85f;
		}
	
		if(shootDistance>=1200&&shootDistance<1300)shootRaps=45.8f+(shootDistance-1200)*0.016f;
			else if(shootDistance<1400)shootRaps=47.4f+(shootDistance-1300)*0.018f;
			else if(shootDistance<1500)shootRaps=49.2f+(shootDistance-1400)*0.014f;
			else if(shootDistance<1600)shootRaps=50.6f+(shootDistance-1500)*0.016f;
			else if(shootDistance<1700)shootRaps=52.2f+(shootDistance-1600)*0.014f;
			else if(shootDistance<1900)shootRaps=53.6f+(shootDistance-1700)*0.018f;
			else if(shootDistance<2300)shootRaps=57.2f+(shootDistance-1900)*0.012f;
			else if(shootDistance<2500)shootRaps=62.f+(shootDistance-2300)*0.014f;
			else if(shootDistance<2700)shootRaps=64.8f+(shootDistance-2500)*0.013f;
			else if(shootDistance<2800)shootRaps=67.4f+(shootDistance-2700)*0.012f;
			else if(shootDistance<3000)shootRaps=68.6f+(shootDistance-2800)*0.006f;
			else if(shootDistance<3300)shootRaps=69.8f+(shootDistance-3000)*0.01f;
			else if(shootDistance<3800)shootRaps=72.8f+(shootDistance-3300)*0.012f;
			else if(shootDistance<4300)shootRaps=78.8f+(shootDistance-3800)*0.01f;
			else if(shootDistance<4500)shootRaps=83.8f+(shootDistance-4300)*0.009f;
			else if(shootDistance<4600)shootRaps=85.6f+(shootDistance-4500)*0.011f;
			else shootRaps=(-9.744e-7*shootDistance*shootDistance)+0.01734f*shootDistance+27.05f;
	}
	else if(USART_RX[0]=='4')
	{
		if(keyRedBlueStatus==0)
		{
			RedFour();
		shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
		shootDistance+=358.85f;
		}
		else
		{
			RedFour();
		shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
		shootDistance+=358.85f;
		}
		
		if(shootDistance>=1400&&shootDistance<1500)shootRaps=51.2f+(shootDistance-1400)*0.018f;
			else if(shootDistance<1700)shootRaps=53.0f+(shootDistance-1500)*0.012f;
			else if(shootDistance<1800)shootRaps=55.4f+(shootDistance-1600)*0.022f;
			else if(shootDistance<1900)shootRaps=57.6f+(shootDistance-1600)*0.016f;
			else if(shootDistance<2000)shootRaps=59.2f+(shootDistance-1900)*0.014f;
			else if(shootDistance<2100)shootRaps=60.6f+(shootDistance-2000)*0.012f;
			else if(shootDistance<2200)shootRaps=61.8f+(shootDistance-2100)*0.01f;
			else if(shootDistance<2300)shootRaps=62.8f+(shootDistance-2200)*0.014f;
			else if(shootDistance<2400)shootRaps=64.2f+(shootDistance-2300)*0.012f;
			else if(shootDistance<3000)shootRaps=65.4f+(shootDistance-2400)*0.01f;
			else if(shootDistance<3100)shootRaps=71.4f+(shootDistance-3000)*0.012f;
			else if(shootDistance<3200)shootRaps=72.6f+(shootDistance-3100)*0.014f;
			else if(shootDistance<3400)shootRaps=74.0f+(shootDistance-3200)*0.012f;
			else if(shootDistance<3500)shootRaps=76.4f+(shootDistance-3400)*0.006f;
			else if(shootDistance<3600)shootRaps=77.0f+(shootDistance-3500)*0.008f;
			else if(shootDistance<3800)shootRaps=77.8f+(shootDistance-3600)*0.011f;
			else if(shootDistance<3900)shootRaps=80.0f+(shootDistance-3800)*0.01f;
			else if(shootDistance<4000)shootRaps=81.0f+(shootDistance-3900)*0.008f;
			else if(shootDistance<4100)shootRaps=81.8f+(shootDistance-4000)*0.01f;
			else if(shootDistance<4300)shootRaps=82.8f+(shootDistance-4100)*0.009f;
			else if(shootDistance<4500)shootRaps=84.6f+(shootDistance-4300)*0.01f;
			else shootRaps=(-9.787e-7*shootDistance*shootDistance)+0.01687f*shootDistance+30.12f;
	}
	ShooterVelCtrl(shootRaps);
}
